{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8e613a7c",
   "metadata": {
    "hideCode": false
   },
   "outputs": [],
   "source": [
    "from grand import Topography, Reference, geoid_undulation\n",
    "from grand import topography\n",
    "from grand import ECEF, Geodetic, LTP, GRANDCS, CartesianRepresentation\n",
    "import numpy as np"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "0fe7069a",
   "metadata": {
    "hideCode": true
   },
   "outputs": [],
   "source": [
    "# Points delimiting GP300 array\n",
    "latitude = np.array(\n",
    "    [\n",
    "        41.4484450740,\n",
    "        41.3240186504,\n",
    "        41.2682974353,\n",
    "        41.2699858577,\n",
    "        41.3599099238,\n",
    "        41.4116665473,\n",
    "        41.4484450740,\n",
    "    ]\n",
    ")\n",
    "longitude = np.array(\n",
    "    [\n",
    "        96.6878480765,\n",
    "        96.6703102286,\n",
    "        96.5824288586,\n",
    "        96.5302861717,\n",
    "        96.4543304128,\n",
    "        96.5113155088,\n",
    "        96.6878480765,\n",
    "    ]\n",
    ")\n",
    "height = np.zeros(len(latitude))\n",
    "subeiAllg = Geodetic(latitude=latitude, longitude=longitude, height=height)\n",
    "subeiDg = Geodetic(\n",
    "    latitude=41.2699858577, longitude=96.5302861717, height=0\n",
    ")  # This one is used as the reference of GRAND ref\n",
    "\n",
    "# GP300 layout to GRAND coordinates\n",
    "subeiAllG = GRANDCS(subeiAllg, location=subeiDg)\n",
    "\n",
    "# update data\n",
    "topography.update_data(coordinates=subeiDg, radius=1e4)  # radius in meter"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7ffe1143",
   "metadata": {
    "hideCode": false,
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "# Change datadir based on your grand location.\n",
    "# datadir = '/Users/rameshkoirala/Documents/GRAND/grand/grand/tools/data/topography'\n",
    "# datadir = '/home/olivier/GRAND/soft/grand/grand/tools/data/topography/'\n",
    "datadir = \"../grand/grand/tools/data/topography/\"\n",
    "topo = Topography(datadir)\n",
    "\n",
    "elev1 = topo.elevation(subeiDg)  # method 1. elevation wrt geoid.\n",
    "elev2 = topography.elevation(subeiDg, \"GEOID\")  # elevation wrt geoid (method 2)\n",
    "elev2b = topography.elevation(subeiDg, \"ELLIPSOID\")  # elevation wrt ellipsoid (method 2)\n",
    "print(\"Elevation of SubeiD.\")\n",
    "print(\"    elevation method1 [wrt geoid] :\", elev1)\n",
    "print(\"    elevation method2 [wrt geoid] :\", elev2)\n",
    "print(\"    elevation [wrt ellipsoid]     :\", elev2b)\n",
    "\n",
    "undu = topography.geoid_undulation(\n",
    "    subeiDg\n",
    ")  # geoid (ie sea level) undulation (departure from ellispoid)\n",
    "print(\"    undulation         :\", undu)\n",
    "\n",
    "# elevation based on LTP and GRAND coordinate system (reference=Reference.LOCAL).\n",
    "ltp = LTP(\n",
    "    x=29000, y=29000, z=0, location=subeiDg, magnetic=True, orientation=\"NWU\"\n",
    ")  # Create GRAND ref by hand\n",
    "gcs1 = GRANDCS(\n",
    "    x=29000, y=29000, z=0, location=subeiDg\n",
    ")  # z-coordinate does not matter. elevation wrt to XY-plane at (x,y).\n",
    "print(\"Elevation based on LTP/GRAND coordinate system.\")\n",
    "print(\"    elevation LTP   [wrt geoid] :\", topography.elevation(ltp))\n",
    "print(\"    elevation GRAND [wrt geoid] :\", topography.elevation(gcs1))\n",
    "print(\"    elevation LTP   [wrt local] :\", topography.elevation(ltp, \"LOCAL\"))\n",
    "print(\"    elevation GRAND [wrt local] :\", topography.elevation(gcs1, \"LOCAL\"))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "9fb1c05b",
   "metadata": {
    "hideCode": false
   },
   "outputs": [],
   "source": [
    "# Get the local ground elevation over a grid and plot it.\n",
    "\n",
    "# Build a meshgrid for ground topography\n",
    "distm = 29000\n",
    "xmin = -distm\n",
    "xmax = +distm\n",
    "ymin = -distm\n",
    "ymax = +distm\n",
    "nsteps = 401  # nb of topo steps\n",
    "x = np.linspace(xmin, xmax, nsteps)\n",
    "y = np.linspace(ymin, ymax, nsteps)\n",
    "X, Y = np.meshgrid(x, y)\n",
    "ground_grid = GRANDCS(x=X.flatten(), y=Y.flatten(), z=np.zeros(X.size), location=subeiDg)\n",
    "\n",
    "# Calculate elevation wrt geoid and GRAND coordinate frame.\n",
    "zg = topography.elevation(\n",
    "    ground_grid, \"GEOID\"\n",
    ")  # Fetch elevation data wrt sea level. reference=GEOID.\n",
    "zG = topography.elevation(ground_grid, \"LOCAL\")  # Fetch elevation data wrt GRAND frame.\n",
    "\n",
    "zg = zg.reshape(X.shape)  # Format as matrix\n",
    "zG = zG.reshape(X.shape)  # Format as matrix\n",
    "deltaz = zg - zG\n",
    "# Difference between geodetic height and z_GRAND (= Earth curvature)\n",
    "\n",
    "# Now go to Geodetic coordinates\n",
    "ground_grid_geo = Geodetic(ground_grid)\n",
    "\n",
    "# Now reformat output\n",
    "lat = ground_grid_geo.latitude.reshape(X.shape)  # Format as matrix\n",
    "lat = lat[0]  # Grab first line\n",
    "lon = ground_grid_geo.longitude.reshape(X.shape)  # Format as matrix\n",
    "lon = lon[:, 0]  # Grab first col"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "24d27fa4",
   "metadata": {
    "hideCode": false,
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "# Now plot the terrain\n",
    "from matplotlib import cm, colors, pyplot as pl\n",
    "from matplotlib.ticker import LinearLocator, FormatStrFormatter\n",
    "\n",
    "params = {\n",
    "    \"legend.fontsize\": 16,\n",
    "    \"axes.labelsize\": 18,\n",
    "    \"axes.titlesize\": 23,\n",
    "    \"xtick.labelsize\": 18,\n",
    "    \"ytick.labelsize\": 18,\n",
    "    \"figure.figsize\": (10, 8),\n",
    "    \"axes.grid\": False,\n",
    "    \"pcolor.shading\": \"auto\",\n",
    "}\n",
    "pl.rcParams.update(params)\n",
    "\n",
    "norm = colors.Normalize(vmin=np.amin(zG), vmax=np.amax(zg))\n",
    "# m    = cm.ScalarMappable(cmap='terrain',norm=norm)\n",
    "\n",
    "# Just a few tests to better understand the subtilities of topography.elevation\n",
    "# for inputs in GRAND OR Geodetic coords.\n",
    "\n",
    "# Plot with z = Geodetic z\n",
    "pl.figure()\n",
    "pl.pcolor(x, y, zg, cmap=\"terrain\", alpha=0.75, norm=norm)\n",
    "pl.plot(subeiAllG.x, subeiAllG.y, \"or-\")\n",
    "pl.xlabel(\"Northing (m)\")\n",
    "pl.ylabel(\"Westing (m)\")\n",
    "pl.colorbar(label=\"Altitude asl (m)\")\n",
    "\n",
    "# Topography plot x = North, y = West\n",
    "pl.figure()\n",
    "pl.pcolor(x, y, zG, cmap=\"terrain\", alpha=0.75, norm=norm)\n",
    "pl.plot(subeiAllG.x, subeiAllG.y, \"or-\")\n",
    "pl.xlabel(\"Northing (m)\")\n",
    "pl.ylabel(\"Westing (m)\")\n",
    "pl.colorbar(label=\"z_GRAND (m)\")\n",
    "# pl.savefig('/Users/rameshkoirala/Documents/GRAND/grandlib/Plots/topography_subei.png', bbox_inches='tight')\n",
    "\n",
    "\n",
    "# Topography plot x = East, y = North (standard)\n",
    "pl.figure()\n",
    "pl.pcolor(-y, x, zG.T, cmap=\"terrain\", alpha=0.75, norm=norm)\n",
    "pl.plot(-subeiAllG.y, subeiAllG.x, \"or-\")\n",
    "pl.xlabel(\"Easting (m)\")\n",
    "pl.ylabel(\"Northing (m)\")\n",
    "pl.colorbar(label=\"z_GRAND (m)\")\n",
    "\n",
    "# Plot in Goedetic coordinates (z = GRAND height)\n",
    "pl.figure()\n",
    "pl.pcolor(lon, lat, zG.T, cmap=\"terrain\", alpha=0.75, norm=norm)\n",
    "pl.plot(subeiAllg.longitude, subeiAllg.latitude, \"or-\")\n",
    "pl.xlabel(\"Longitude (deg)\")\n",
    "pl.ylabel(\"Latitude (deg)\")\n",
    "pl.colorbar(label=\"z_GRAND (m)\")\n",
    "\n",
    "pl.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7f09f4ee",
   "metadata": {
    "hideCode": true
   },
   "outputs": [],
   "source": [
    "# Difference with GRAND altitudes = Earth curvature\n",
    "norm = colors.Normalize(vmin=np.amin(deltaz), vmax=np.amax(deltaz))\n",
    "m = cm.ScalarMappable(cmap=\"terrain\", norm=norm)\n",
    "\n",
    "pl.figure()\n",
    "pl.pcolor(x, y, deltaz, cmap=\"terrain\", alpha=0.75)\n",
    "pl.xlabel(\"Northing (m)\")\n",
    "pl.ylabel(\"Westing (m)\")\n",
    "bar = pl.colorbar(m)\n",
    "bar.set_label(\"$\\Delta$Height (m)\")\n",
    "# pl.savefig('/Users/rameshkoirala/Documents/GRAND/grandlib/Plots/earth_curvature.png', bbox_inches='tight')\n",
    "\n",
    "pl.figure()\n",
    "pl.plot(x, deltaz[0], \"teal\", label=\"top slice\", lw=2)\n",
    "pl.plot(x, deltaz[round(nsteps / 2)], \"orange\", label=\"center slice\", lw=2)\n",
    "pl.xlabel(\"Northing (m)\")\n",
    "pl.ylabel(\"$\\Delta$Height (m)\")\n",
    "pl.legend()\n",
    "pl.grid(ls=\"--\", alpha=0.3)\n",
    "pl.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d188f085",
   "metadata": {
    "hideCode": false
   },
   "outputs": [],
   "source": [
    "# Now check when a given trajectory meets ground\n",
    "# Does not match with distance function... To be solved.\n",
    "\n",
    "\n",
    "def arrow(x, y):\n",
    "    dx, dy = x[-1] - x[0], y[-1] - y[0]\n",
    "    arrow_l = np.sqrt(dx ** 2 + dy ** 2)\n",
    "    return x[0], y[0], dx, dy, arrow_l\n",
    "\n",
    "\n",
    "# direction = CartesianRepresentation(x=100, y=10, z=-0.1)   # Traj = direction vector in GRAND ref\n",
    "direction = GRANDCS(x=100, y=10, z=-0.1, location=subeiDg)\n",
    "x0 = GRANDCS(x=-8000, y=12000, z=2200, location=subeiDg)  # Initial point along traj (in GRAND ref)\n",
    "dirE = np.matmul(\n",
    "    x0.basis.T, direction\n",
    ")  # GRAND --> ECEF. input direction must be in ECEF. ToDo: Fix this.\n",
    "distance1 = topography.distance(x0, dirE)  # direction vector here is in ECEF frame.\n",
    "# direction = np.matmul(x0.basis, dirE)    # ECEF --> GRAND. change direction vector back to GRAND frame.\n",
    "print(\"Distance to ground:\", distance1)\n",
    "\n",
    "# Build traj\n",
    "u = np.linspace(0, 3.0e4, 501)  # Distance from x0 (meters)\n",
    "dirn = direction / np.linalg.norm(direction)  # normalize direction vector\n",
    "traj = dirn * u + x0\n",
    "\n",
    "# Get elevation below traj\n",
    "traj_grand = GRANDCS(x=traj[0], y=traj[1], z=traj[2], location=subeiDg)  # Compute traj coordinates\n",
    "ztG = topography.elevation(traj_grand, reference=\"LOCAL\")  # z-coordinate of ground in GRAND ref\n",
    "\n",
    "icrash = np.argmin(abs(u - distance1))  # index of trajectory closest to intersection to ground\n",
    "\n",
    "# Side view\n",
    "pl.figure()\n",
    "ka, kha, dka, dkha, arrow_l = arrow(\n",
    "    u[: icrash + int(0.15 * len(u))], traj[2][: icrash + int(0.15 * len(u))]\n",
    ")\n",
    "pl.arrow(\n",
    "    ka,\n",
    "    kha,\n",
    "    dka,\n",
    "    dkha,\n",
    "    width=0.2,\n",
    "    head_length=20 * np.diff(u)[0],\n",
    "    head_width=200 * np.diff(traj[2])[0],\n",
    "    facecolor=\"k\",\n",
    ")\n",
    "pl.plot(u, ztG, \"olive\", lw=2)  # ground\n",
    "pl.plot(u[icrash], traj[2, icrash], \"*k\", markersize=15)  # intersection with ground\n",
    "pl.plot(0, x0.z, \"or\", markersize=14)  # initial point\n",
    "pl.xlabel(\"Track (m)\")\n",
    "pl.ylabel(\"z_GRAND (m)\")\n",
    "pl.grid(ls=\"--\", alpha=0.3)\n",
    "# pl.savefig('/Users/rameshkoirala/Documents/GRAND/grandlib/Plots/topography_along_direction.png')\n",
    "\n",
    "\n",
    "# Top view\n",
    "pl.figure(figsize=(10, 8))\n",
    "norm = colors.Normalize(vmin=np.amin(zg), vmax=np.amax(zg))\n",
    "m = cm.ScalarMappable(cmap=\"terrain\", norm=norm)\n",
    "pl.pcolor(x, y, zG, cmap=\"terrain\", alpha=0.75)\n",
    "ka, kha, dka, dkha, arrow_l = arrow(\n",
    "    traj[0][: icrash + int(0.15 * len(u))], traj[1][: icrash + int(0.15 * len(u))]\n",
    ")\n",
    "pl.arrow(\n",
    "    ka,\n",
    "    kha,\n",
    "    dka,\n",
    "    dkha,\n",
    "    # width=6, head_length=.1*arrow_l, head_width=0.08*arrow_l,\n",
    "    width=6,\n",
    "    head_length=20 * np.diff(traj[0])[0],\n",
    "    head_width=200 * np.diff(traj[1])[0],\n",
    "    facecolor=\"k\",\n",
    ")\n",
    "pl.plot(x0.x, x0.y, \"or\", markersize=14)\n",
    "pl.plot(traj[0, icrash], traj[1, icrash], \"*k\", markersize=16)  # intersection with ground\n",
    "pl.xlabel(\"Northing (m)\")\n",
    "pl.ylabel(\"Westing (m)\")\n",
    "bar = pl.colorbar(m)\n",
    "bar.set_label(\"z_GRAND (m)\")\n",
    "# pl.savefig('/Users/rameshkoirala/Documents/GRAND/grandlib/Plots/topography_and_direction.png')\n",
    "pl.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d878ae07",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "celltoolbar": "Hide code",
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.10"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
